#include <Arduino.h>
#include <stddef.h>
#include <stdlib.h>
#include <stdint.h>
#include "maxim.h"
#include "utils.h"
#include "Printbuf.h"
#include "registers.h"
#include "packdata.h"
#include <math.h>
/*
 * ================== Declare Statics here
 */
PackData* PackData::loneSingleton = NULL;

uint8_t DeviceData::totCells;
uint16_t DeviceData::minVoltage;
uint16_t DeviceData::maxVoltage;
uint16_t DeviceData::avgVoltage;
uint32_t DeviceData::totVoltage;


// Need to think about cell id's.
//
// BUGBUG: numCells is funky. It should only be set after setting
// all voltages.
//
// cellId is the (device+cell) Id 
// cellNumber is simply the number for a device.
// Both cellId and cellNumber are 1-based
//

DeviceData::DeviceData(uint8_t d, PackData* pd) 
{
  deviceId=d; 
  pack=pd;
  cellOffsetId = 0;
  numCells = 0;
  for (uint8_t i=0;i<MAXDEVICECELLS;i++) {
    cellVoltages[i] = 0;
  }
}

void DeviceData::setCellVoltage(uint8_t cellNumber, uint16_t voltage) {
  //assert(cellNumber > 0 && cellNumber <= MAXDEVICECELLS);
  cellVoltages[cellNumber-1] = voltage;
}

float DeviceData::getCellVoltage(uint8_t cellNumber) {
  //assert(cellNumber > 0 && cellNumber <= MAXDEVICECELLS);
  return (cellVoltages[cellNumber-1] * ADC_VOLTAGE_RESOLUTION);
}

void DeviceData::setCellOffsetId(uint16_t offset) {
  cellOffsetId = offset;
}

boolean DeviceData::disableNullCells(uint16_t& startingCellId,
                                     uint16_t& cellMask) {
  cellOffsetId = startingCellId;
  RegCellen::setEnableAllCellMask(cellMask);

  for (uint8_t i=0;i<MAXDEVICECELLS;i++) {
    if (cellVoltages[i] > ZERO_VOLTAGE_RESOLUTION_UNITS) {
      numCells++;
    } else {
      RegCellen::setDisableCellMask(cellMask,i+1);
    }
  }
  enabledCellsMask = cellMask;
  // Serial.print("Cell Mask:");Serial.println(cellMask,HEX);
  startingCellId += numCells;
  return true;
}

void sfprintf(char* buf, int len, float f) 
{
  // know format is "X.XX"
  int whole = (int)f;
  int decimal = round((f-whole)*100);
  snprintf(buf,len,"%d.%.2d",whole,decimal);
}

void DeviceData::printHdr()
{
  PrintBuf::singleton()->add("<cells>",PrintBuf::SPACE); 
}
void DeviceData::printTrailer()
{
  PrintBuf::singleton()->add(""); // for EOL
  PrintBuf::singleton()->print();
}

// HACK: PrintBuf only supports 56 chars. So, print 4 cells
// at a time (max = 9 chars for "cell# value")
void DeviceData::printCellVoltages(boolean filterNulls) 
{
  bool noHdr = true;
  char buf[16];
  int j=0;
  for (uint8_t i=1;i<=MAXDEVICECELLS;i++) {

    if (filterNulls &&
        RegCellen::isCellDisabled(enabledCellsMask,i)) {
      continue;
    }

    if (noHdr) {
      printHdr();
      noHdr = false;
    }

    PrintBuf::singleton()->add(cellOffsetId+j,PrintBuf::SPACE);
    float v = cellVoltages[i-1]*ADC_VOLTAGE_RESOLUTION;
    sfprintf(buf,sizeof(buf),v);
    PrintBuf::singleton()->add(buf,PrintBuf::SPACE);
    ++j;
    if (j%4 == 0) {
      printTrailer();
      noHdr = true;
    }
  }

  if (j && j%4 != 0) printTrailer();
}

void DeviceData::printUVcellVoltages()
{
  bool noHdr = true;
  char buf[6];
  int j=0,k=0;
  for (uint8_t i=1;i<=MAXDEVICECELLS;i++) {
    if (RegCellen::isCellDisabled(enabledCellsMask,i)) {
      continue;
    }
    if (cellVoltages[i-1] < RegUVthrClear::ThrValue) {

      if (noHdr) {
	printHdr();
	noHdr = false;
      }

      PrintBuf::singleton()->add(cellOffsetId+j,PrintBuf::SPACE);
      float v = cellVoltages[i-1]*ADC_VOLTAGE_RESOLUTION;
      sfprintf(buf,sizeof(buf),v);
      PrintBuf::singleton()->add(buf,PrintBuf::SPACE);
      ++k;
      if (k%4 == 0) {
	printTrailer();
	noHdr = true;
      }
    }
    j++;
  }
  if (k && k%4 != 0) printTrailer();
}

void DeviceData::initStatCellVoltages()
{
  minVoltage = 0x0FFF;
  maxVoltage = 0;
  totVoltage = 0;
  totCells = 0;
}

void DeviceData::setStatCellVoltages()
{
  for (uint8_t i=1;i<=MAXDEVICECELLS;i++) {
    if (RegCellen::isCellDisabled(enabledCellsMask,i)) {
      continue;
    }
    uint16_t v = cellVoltages[i-1];

    if ( v < minVoltage) {
      minVoltage = v;
    }
    if (v > maxVoltage) {
      maxVoltage = v;
    }
    totVoltage += v;
  }
  totCells += numCells;

  // final average will only be calculated by the last device.
  if (totCells) {
    avgVoltage = totVoltage / totCells;
  }
}

void DeviceData::printStatCellVoltages()
{
  Serial.print("Total:");
  Serial.println(totCells,DEC);
  Serial.print("Min:");
  Serial.println(minVoltage*ADC_VOLTAGE_RESOLUTION,3);
  Serial.print("Avg:");
  Serial.println(avgVoltage*ADC_VOLTAGE_RESOLUTION,3);
  Serial.print("Total:");
  Serial.println(totVoltage*ADC_VOLTAGE_RESOLUTION,3);
}

PackData::PackData() {
  numDevices = 0;
  maxCells = 0;

  for (uint8_t i=0;i<MAXDEVICES;i++) {
    deviceData[i] = 0;
  }
  loneSingleton = this;
}

PackData::~PackData() {
  clear();
}
  
void PackData::clear() {
  for (uint8_t i=0; i<MAXDEVICES;i++) {
    delete deviceData[i];
  }
  numDevices=0;
  maxCells=0;
}

void PackData::setNumDevices(uint8_t nd) {
  //assert(nd<MAXDEVICES);
  numDevices = nd;
  for (uint8_t i=0;i<numDevices;i++) {
    deviceData[i] = new DeviceData(i+1,this);
  }
}

uint8_t PackData::getNumDevices() {return numDevices;}

void PackData::setCellData(RegCellData& cellData) {
  for (uint8_t i=0;i<numDevices;i++) {
    deviceData[i]->setCellVoltage(cellData.getCellNumber(),
                                  cellData.getVoltageValue(i+1));
  }
}

// Print number of cells in pack.
void PackData::printNumCells()
{
  PrintBuf::singleton()->add("<count>",PrintBuf::SPACE);
  PrintBuf::singleton()->add(maxCells);
  PrintBuf::singleton()->print();
}

// Print all cell voltages
void PackData::printCellVoltages(boolean filterNoise) {
  for (uint8_t i=0;i<numDevices;i++) {
    deviceData[i]->printCellVoltages(filterNoise);
  }
}

// Print all 
void PackData::print() {
  printCellVoltages(false);
  Serial.print("Num Devices: "); Serial.println(numDevices,DEC);
  Serial.print("Max Cells: "); Serial.println(maxCells,DEC);
}

// Print UV cell voltages
// same HACK as printCellVoltages
void PackData::printUVcellVoltages() {
  for (uint8_t i=0;i<numDevices;i++) {
    deviceData[i]->printUVcellVoltages();
  }
}

// Print stats for cell voltages
void PackData::printStatCellVoltages() {
  DeviceData::initStatCellVoltages();
  for (uint8_t i=0;i<numDevices;i++) {
    deviceData[i]->setStatCellVoltages();
  }
  DeviceData::printStatCellVoltages();
}

// Disable null cells by disabling scanning for those cells.
// ALso set all alert enables.
boolean PackData::disableNullCells()
{
  RegCellen regCellen(numDevices);
  RegAlrtUVen regAlrtUVen(numDevices);
  uint16_t cellMask;
  uint16_t cellOffsetId = 1;

  for (uint8_t dn=0;dn<numDevices;dn++) {
    if (!deviceData[dn]->disableNullCells(cellOffsetId,
                                          cellMask)) {
      return false;
    }
                                           
    // Serial.print("Cell Mask=0x");Serial.println(cellMask,HEX);
    if (!regCellen.enableCells(cellMask,dn+1)) {
      return false;
    }

    // Set only Undervoltage alert for now.
    if (!regAlrtUVen.enableCells(cellMask,dn+1)) {
      return false;
    }
  }
  maxCells = cellOffsetId-1;
  return true;
}

boolean PackData::scan()
{
  // initiate scanning.
  RegScanctrl regScanctrl(numDevices);
  return regScanctrl.enable();
}

boolean PackData::isDeviceUV()
{
  RegStatus regStatus(numDevices);
  if (!regStatus.read()) {
    return false;
  }

  return regStatus.isUnderVoltage();
}

// Read all cell voltages
boolean PackData::readCellVoltages()
{
  RegCellData regCellData(numDevices);
  for (uint8_t cn=1; cn<=12; cn++) {

    // initialize
    regCellData.initCellNumber(cn);

    // read from device
    if (!regCellData.readFromDevice()) {
      PrintBuf::singleton()->add("readFromDevice!cell: ", ':');
      PrintBuf::singleton()->add(cn);
      return false;
    }

    // Add to Pack data
    setCellData(regCellData);
  }
  
  return true;
}


